#include <stdio.h>
#include <stdlib.h>
#include "queue.h"


Queue* initQueue()
{
   Queue* queue = (Queue*)malloc(sizeof(Queue));
   queue->head = 0;
   queue->count = 0;

   if( pthread_mutex_init(&(queue->mutex),NULL) )
   {
       fprintf(stderr, "Unable to initialize a mutex\n");
       exit(EXIT_FAILURE);
   }		       

   /* create binary semaphores set to locked 
    * first 0 is for sem between threads
    * */
   if(sem_init(&(queue->read_delay), 0, 0))
   {
        printf("Could not initialize a semaphore\n");
        exit(EXIT_FAILURE);
   }
   if(sem_init(&(queue->write_delay), 0, MAX_QSIZE))
   {
        printf("Could not initialize a semaphore\n");
        exit(EXIT_FAILURE);
   }

   if(DEBUG) printf("DEBUG: Q init complete\n");
   return queue;
}

int enqueue(Queue* queue, Element* element)
{
   int end;

   pthread_mutex_lock(&(queue->mutex));
   
   /* check if full */
   if(MAX_QSIZE == queue->count) 
   {
      pthread_mutex_unlock(&(queue->mutex));

      /* queue full */
      if(DEBUG) printf("DEBUG: Queue full\n");
      sem_wait(&(queue->write_delay));

      pthread_mutex_lock(&(queue->mutex)); /* reacquire mutex */
   } else 
   {
      /* this should never wait since
       * we know empty slot exists 
       */
      sem_wait(&(queue->write_delay));
   }

   /* calculate end and wrap if needed */
   end = (queue->head + queue->count) % MAX_QSIZE;
   queue->queue_array[end] = element;
   queue->count++;

   pthread_mutex_unlock(&(queue->mutex));
   sem_post(&(queue->read_delay)); /* let reader read */ 

   return 0;
}

Element* dequeue(Queue* queue)
{
   Element* element;

   pthread_mutex_lock(&(queue->mutex));
  
   /* check if empty queue */
   if(0 == queue->count)
   {
      pthread_mutex_unlock(&(queue->mutex));
      
      if(DEBUG) printf("DEBUG: Queue empty\n");
      sem_wait(&(queue->read_delay));

      pthread_mutex_lock(&(queue->mutex)); /* reaquire mutex */
   } else 
   {
      /* this should never block  as there
       * must be data in the queue */
      sem_wait(&(queue->read_delay));
   }

   element = queue->queue_array[queue->head];
   /* calcluate new head and wrap if needed */
   queue->head = (queue->head + 1) % MAX_QSIZE;
   queue->count--;
  
   pthread_mutex_unlock(&(queue->mutex));
   sem_post(&(queue->write_delay)); /* let writer write */
   return element;
}
